#region Using Statements
using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;
using JigLibX.Geometry;
using JigLibX.Math;
#endregion

namespace JigLibX.Collision
{
    public class CollDetectBoxHeightmap : DetectFunctor
    {

        private List<CollPointInfo> collPts = new List<CollPointInfo>(8);

        public CollDetectBoxHeightmap()
            : base("BoxHeighmap", (int)PrimitiveType.Box, (int)PrimitiveType.Heightmap)
        {
        }

        public override void CollDetect(CollDetectInfo infoOrig, float collTolerance, CollisionFunctor collisionFunctor)
        {
            CollDetectInfo info = infoOrig;
            if (info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0).Type == this.Type1)
            {
                CollisionSkin skinSwap = info.Skin0;
                info.Skin0 = info.Skin1;
                info.Skin1 = skinSwap;
                int primSwap = info.IndexPrim0;
                info.IndexPrim0 = info.IndexPrim1;
                info.IndexPrim1 = primSwap;
            }

            Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero;
            Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero;


            // todo - proper swept test
            Box oldBox = (Box) info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0);
            Box newBox = (Box) info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0);

            Heightmap oldHeightmap = (Heightmap) info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1);
            Heightmap newHeightmap = (Heightmap)info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1);

            Vector3[] oldPts, newPts;
            oldBox.GetCornerPoints(out oldPts);
            newBox.GetCornerPoints(out newPts);

            collPts.Clear();

            Vector3 collNormal = Vector3.Zero;

            for (int i = 0; i < 8; ++i)
            {
                Vector3 newPt = newPts[i];
                float newDist;
                Vector3 normal;
                newHeightmap.GetHeightAndNormal(out newDist, out normal, newPt);

                if (newDist < collTolerance)
                {
                    Vector3 oldPt = oldPts[i];
                    float oldDist = oldHeightmap.GetHeight(oldPt);
                    collPts.Add(new CollPointInfo(oldPt - body0Pos, oldPt - body1Pos, -oldDist));
                    collNormal += normal;
                }
            }


            if (collPts.Count > 0)
            {
                if (collNormal.LengthSquared() > 0.0f)
                    collNormal.Normalize();
                collisionFunctor.CollisionNotify(info, collNormal, collPts);
            }

        }






    }
}
